#include <stdio.h>
#include <stdlib.h>
#include <memory.h>
#include <stdint.h>
#include <string.h>
#include <unistd.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "iot_i2c.h"
#include "iot_errno.h"
#include "hi_io.h"
#include "hi_time.h"
#include "hi_adc.h"
#include "iot_errno.h"
#include "tank_sg90.h"
#include "tank_control.h"
#include "tank_oled_ssd1306.h"
#include "led.h"
#include "app_demo_uart_tank.h"

//#define     GPIO5 5
#define     GPIO14 14    //定义激光接口
#define     COUNT 10
#define GPIOFUNC 0
//#define     FUNC_GPIO 0
//#define     IOT_IO_PULL_UP 1
//#define     VLT_MIN                     (100)
//#define     OLED_FALG_ON                ((unsigned char)0x01)
//#define     OLED_FALG_OFF               ((unsigned char)0x00)
//#define AHT20_BAUDRATE (400 * 1000)
//#define AHT20_I2C_IDX 0

//unsigned short  g_gpio5_adc_buf[ADC_TEST_LENGTH] = {0 };
//unsigned int    g_gpio5_tick = 0;

int g_tank_status = TANK_RECONNOITRE_STATUS;    //坦克默认侦察状态
// unsigned int    g_tank_flag = 1;    //接收到的flag

/*
void switch_init(void)  //开关初始化
{
    IoTGpioInit(GPIO5);
    hi_io_set_func(GPIO5, FUNC_GPIO);
    IoTGpioSetDir(GPIO5, IOT_GPIO_DIR_IN);
    hi_io_set_pull(GPIO5, IOT_IO_PULL_UP);
}

static void Oled1(void)
{

    OledFillScreen(0);
    //IoTI2cInit(AHT20_I2C_IDX, AHT20_BAUDRATE);
    OledShowString(20, 3, "reconnoitre", 1); //屏幕第20列3行显示1行
}

static void Oled2(void)
{

    OledFillScreen(0);
    //IoTI2cInit(AHT20_I2C_IDX, AHT20_BAUDRATE);
    OledShowString(20, 4, "stopshoot", 1); //屏幕第20列4行显示1行
}

static void Oled_Task(void)
{
    if (g_tank_status == TANK_RECONNOITRE_STATUS){
        Oled1();
    }else if (g_tank_status == TANK_STOP_SHOOT_STATUS){
        Oled2();
    }
}

void gpio5_isr_tankfunc_mode(void)  //自定义坦克中断服务处理功能函数
{
    printf("gpio5_isr_tankfunc_mode start\n");
    unsigned int tick_interval = 0;  //按键间隔
    unsigned int current_gpio5_tick = 0;   //gpio5的当前按键值

    current_gpio5_tick = hi_get_tick();    //获取当前按键状态
    tick_interval = current_gpio5_tick - g_gpio5_tick;  //按键间隔=当前按键值-初始化按键值
    
    if (tick_interval < KEY_INTERRUPT_PROTECT_TIME)  //如果按键间隔小于按键中断时间则返回空值
    {
        return NULL;
    }
    g_gpio5_tick = current_gpio5_tick;   //更新按键值为当前按键值
    
    //按键切换模式
    if (g_tank_status == TANK_RECONNOITRE_STATUS) 
    {
        g_tank_status = TANK_STOP_SHOOT_STATUS;    //坦克停止射击
        printf("stop and shoot\n");
        
    } 
    else if (g_tank_status == TANK_STOP_SHOOT_STATUS)  
    {
        g_tank_status = TANK_RECONNOITRE_STATUS;   // 坦克识别
        printf("reconnoitre\n");
    }
}

unsigned char get_gpio5_voltage(void *param)  //配置gpio5引脚电压参数
{
    printf("get_gpio5_voltage start\n");
    int i;
    unsigned short data;
    unsigned int ret;
    unsigned short vlt;
    float voltage;
    float vlt_max = 0;
    float vlt_min = VLT_MIN;
    float a = 1.8;
    float c = 4096.0;
    int b = 4;
    float vlt_1 = 0.6;
    float vlt_2 = 1.0;

    hi_unref_param(param);
    memset_s(g_gpio5_adc_buf, sizeof(g_gpio5_adc_buf), 0x0, sizeof(g_gpio5_adc_buf));
    for (i = 0; i < ADC_TEST_LENGTH; i++) {
        ret = hi_adc_read(HI_ADC_CHANNEL_2, &data, HI_ADC_EQU_MODEL_4, HI_ADC_CUR_BAIS_DEFAULT, 0xF0);
		// ADC_Channal_2  自动识别模式  CNcomment:4次平均算法模式 CNend
        if (ret != IOT_SUCCESS) {
            printf("ADC Read Fail\n");
            return  NULL;
        }
        g_gpio5_adc_buf[i] = data;
    }

    for (i = 0; i < ADC_TEST_LENGTH; i++) {
        vlt = g_gpio5_adc_buf[i];
        voltage = (float)vlt * a * b / c;
		//vlt * 1.8* 4 / 4096.0为将码字转换为电压
        vlt_max = (voltage > vlt_max) ? voltage : vlt_max;
        vlt_min = (voltage < vlt_min) ? voltage : vlt_min;
    }
    printf("gpio5_vlt_max is %f\r\n", vlt_max);
    if (vlt_max > vlt_1 && vlt_max < vlt_2) {
        gpio5_isr_tankfunc_mode();    //自定义坦克中断服务处理功能函数
    }
}
*/
void shoot_init(unsigned int  gpio)
{
    hi_io_set_func(gpio, GPIOFUNC);
    IoTGpioSetDir(gpio, IOT_GPIO_DIR_OUT);
    IoTGpioSetOutputVal(gpio, 1);
}
void gpio14_shoot(void)   //自定义激光射击函数
{
    printf("gpio14_shoot start\n");
    unsigned int time = 20000;
    printf("start shoot shoot shoot shoot\n");
    //ledspark();
    shoot_init(GPIO14);
    IoTGpioSetOutputVal(GPIO14, IOT_GPIO_VALUE0);
    hi_udelay(50*time);
    IoTGpioSetOutputVal(GPIO14, IOT_GPIO_VALUE1);
    hi_udelay(25*time);
    IoTGpioSetOutputVal(GPIO14, IOT_GPIO_VALUE0);
    hi_udelay(50*time);
    IoTGpioSetOutputVal(GPIO14, IOT_GPIO_VALUE1);
    hi_udelay(25*time);
}
/*
// 按键中断
void interrupt_monitor(void)
{
    printf("interrupt_monitor start\n");
    unsigned int  ret = 0;
    // gpio5 switch2 mode 
    g_gpio5_tick = hi_get_tick();
    ret = IoTGpioRegisterIsrFunc(GPIO5, IOT_INT_TYPE_EDGE, IOT_GPIO_EDGE_FALL_LEVEL_LOW, get_gpio5_voltage, NULL);
    if (ret == IOT_SUCCESS) {
        printf(" register gpio5\r\n");
    }
}

*/
int get_distance(int x_min,int x_max,int y_min,int y_max)   //自定义测算距离函数
{
    printf("get_distance start\n");
    unsigned int temp = 1000;
    int area = 0;
    int distance = 0;
    area = (x_max - x_min) * (y_max - y_min);
    printf("area= %d",area);
    distance = area / temp;
    printf("distance= %d",distance);
    return distance;
}

static unsigned int tank_get_l_or_r(void)  //自定义坦克转动侦察
{
    printf("tank_get_l_or_r start\n");
    unsigned int time = 20000;
    int left_distance = 0;
    int right_distance = 0;
    g_tank_status = TANK_STOP_SHOOT_STATUS;    //坦克侦察停止并射击 
    
    tank_stop();
    engine_turn_left();
    // tank_left_2();
    hi_udelay(time);
    left_distance = get_distance(x_min,x_max,y_min,y_max);    //get一下left_distance
    printf("left_distance= %d",left_distance);
    engine_middle();   //居中
    hi_udelay(time);
    engine_turn_right();
    hi_udelay(time);
    right_distance = get_distance(x_min,x_max,y_min,y_max);    //get一下right_distance
    printf("right_distance= %d",right_distance);
    engine_middle();   //居中
    hi_udelay(time);
    //ledspark();   //led闪烁
    //hi_udelay(time);
    
    if (left_distance < right_distance) {
        return TANK_GO_LEFT;
    } else {
        return TANK_GO_RIGHT;
    }
    
    // return TANK_GO_LEFT;    //测试时使用
}

//自定义坦克距离判断函数
int tank_forward_status;//电机是否启动

void tank_mode_control(void)
{
    printf("tank_mode_controll start:stop and shoot\n");
    unsigned int time = 20000;
    unsigned int ret = tank_get_l_or_r();
    printf("ret is %ud\r\n", ret);
    tank_stop();
    engine_middle();
    hi_udelay(time);
    if (ret == TANK_GO_LEFT) {
            tank_left();
            hi_udelay(time);
            tank_forward();
            hi_udelay(time);
            tank_stop();
            gpio14_shoot();
            hi_udelay(time);
        } else if (ret == TANK_GO_RIGHT) {
            tank_right();
            hi_udelay(time);
            tank_forward();
            hi_udelay(time);
            tank_stop();
            gpio14_shoot();
            hi_udelay(time);
        }
    g_tank_status = TANK_RECONNOITRE_STATUS;// 坦克侦察识别
    tank_forward_status = 0;//下次巡逻时,小车的电机启动  
    //检测数据复位
    tankControlflag = 0;
    tankflag = 0;
    x_min = 0;
    y_min = 0;
    x_max = 0;
    y_max = 0;
}
//小车运动侦察 逻辑能否更简单些 这样花费的时间比较大
int active;//用来选择每次小车执行的一个行为。 这里一共有9个依次执行的行为
int tank_reconnoitre(void)
{
    unsigned int time = 20000;
    printf("tank_reconnoitre start 111111=====active=%d\n",active);
    int go;
    go = active%9;
    //led_off();
    if(tank_forward_status == 0 ){
        tank_forward(); //电机启动
        hi_udelay(time);
        tank_forward_status = 1;
    }
    if(go == 0){
        active++;
        engine_reco_one();//居中
        hi_udelay(time);
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;  
    }
    if(go == 1){
        active++;
        engine_reco_two(); //往左
        hi_udelay(time);
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;  
    }
    if(go == 2){
        active++;
        engine_reco_three(); //居中
        hi_udelay(time);
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;  
    }
    if(go == 3){
        active++;
        engine_reco_two(); //往右
        hi_udelay(time);
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;       
    }
    if(go == 4){
        active++;
        engine_reco_one();   //居中
        hi_udelay(time);   
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;
    }
    if(go == 5){
        active++;
        engine_reco_five();   
        hi_udelay(time);   
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;
    }
    if(go == 6){
        active++;
        engine_reco_six();   
        hi_udelay(time);   
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;
    }
    if(go == 7){
        active++;
        engine_reco_five();   
        hi_udelay(time);   
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;
    }
    if(go == 8){
        active++;
        engine_reco_one();   
        hi_udelay(time);   
        printf("==================tankControlflag================ %d\n",tankControlflag);
        if (tankControlflag == 1){
            g_tank_status = TANK_STOP_SHOOT_STATUS;
            return g_tank_status;
        }
        printf("tank_reconnoitre.....\n");
        g_tank_status = TANK_RECONNOITRE_STATUS;
        return g_tank_status;
    }
}



/*
void RobotTankTestTask(void)    //自定义坦克任务函数
{
    printf("start\r\n");
    //switch_init(); 
    //interrupt_monitor(); 
    //interrupt_monitor_laser();

    unsigned int time = 20;
    while(1){
        //Oled_Task();
        switch (g_tank_status) {   //坦克状态判断
            case TANK_STOP_SHOOT_STATUS:  //坦克停止侦察并射击
                tank_mode_controll();
                break;
            case TANK_RECONNOITRE_STATUS:   //坦克侦察
                tank_reconnoitre();
                break;
            default:
                break;
        }
        IoTWatchDogDisable();
        // osDelay(time);
        // tankTaskMsleep(4000);
       
    }

}
*/

/*
void *TankOledTestTask(void* param)    //自定义Oled任务函数
{
    printf("switch\r\n");
    switch_init(); 
    interrupt_monitor(); 
    interrupt_monitor_laser();
    OledInit();
    IoTI2cInit(AHT20_I2C_IDX, AHT20_BAUDRATE);

    unsigned int time = 20;
    while(1){
        Oled_Task();
        IoTWatchDogDisable();
        //osDelay(time);
    }
}


*/
// void TankControl(void)
// {
//     // osThreadAttr_t attr;

//     // attr.name = "RobotTankTestTask";
//     // attr.attr_bits = 0U;
//     // attr.cb_mem = NULL;
//     // attr.cb_size = 0U;
//     // attr.stack_mem = NULL;
//     // attr.stack_size = TANK_CONTROL_DEMO_TASK_STAK_SIZE;
//     // attr.priority = TANK_CONTROL_DEMO_TASK_PRIORITY;

//     // if (osThreadNew(RobotTankTestTask, NULL, &attr) == NULL) 
//     // {
//     //     printf("[Ssd1306TestDemo] Falied to create RobotTankTestTask!\n");
//     // }

//     osThreadAttr_t attr;
//     IoTWatchDogDisable();

//     attr.name = "IOTDEMO";
//     attr.attr_bits = 0U;
//     attr.cb_mem = NULL;
//     attr.cb_size = 0U;
//     attr.stack_mem = NULL;
//     attr.stack_size = TANK_CONTROL_DEMO_TASK_STAK_SIZE;
//     attr.priority = TANK_CONTROL_DEMO_TASK_PRIORITY;

//     if (osThreadNew((osThreadFunc_t)RobotTankTestTask, NULL, &attr) == NULL) {
//         printf("[Ssd1306TestDemo] Falied to create RobotTankTestTask!\n");
//     }

// }
// SYS_RUN(TankControl);
/*


void TankOled(void)
{
    osThreadAttr_t attr;

    attr.name = "TankOledTestTask";
    attr.attr_bits = 0U;
    attr.cb_mem = NULL;
    attr.cb_size = 0U;
    attr.stack_mem = NULL;
    attr.stack_size = TANK_CONTROL_DEMO_TASK_STAK_SIZE;
    attr.priority = TANK_CONTROL_DEMO_TASK_PRIORITY;

    if (osThreadNew(TankOledTestTask, NULL, &attr) == NULL) 
    {
        printf("[Ssd1306TestDemo] Falied to create TankOledTestTask!\n");
    }

}
APP_FEATURE_INIT(TankOled);
*/